#include <ros/ros.h>
#include<sensor_msgs/Imu.h>

ros::Publisher imu_data_pub_;

void handle_gps_imu(const sensor_msgs::Imu::ConstPtr& imu_raw){
    sensor_msgs::Imu imu_msgs;
    imu_msgs = *imu_raw;
    imu_msgs.linear_acceleration.z = imu_msgs.linear_acceleration.z +9.805;
    // ROS_DEBUG("handle gps imu");
    imu_data_pub_.publish(imu_msgs);
}

int main(int argc, char **argv){
    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::init(argc, argv, "handle_gps_imu");
    ros::NodeHandle nh;

    ros::Subscriber gps_imu_sub = nh.subscribe("/gps/imu", 100, handle_gps_imu); 
  
    imu_data_pub_ = nh.advertise<sensor_msgs::Imu>("/imu/data", 1,true);

    ros::spin();

    return 0;
}

